#include "tools.h"
//保存相机参数
void saveCameraParameters(const std::string& filename, const pcl::visualization::Camera& camera)
{
    std::ofstream file(filename);
    if (file.is_open())
    {
        file << camera.focal[0] << " " << camera.focal[1] << " " << camera.focal[2] << std::endl;
        file << camera.pos[0] << " " << camera.pos[1] << " " << camera.pos[2] << std::endl;
        file << camera.view[0] << " " << camera.view[1] << " " << camera.view[2] << std::endl;
        file << camera.fovy << std::endl;
        file.close();
    }
}
//导入相机参数
void loadCameraParameters(const std::string& filename, pcl::visualization::Camera& camera)
{
    std::ifstream file(filename);
    if (file.is_open())
    {
        file >> camera.focal[0] >> camera.focal[1] >> camera.focal[2];
        file >> camera.pos[0] >> camera.pos[1] >> camera.pos[2];
        file >> camera.view[0] >> camera.view[1] >> camera.view[2];
        file >> camera.fovy;
        file.close();
    }
}

//可视化显示点云数据工具
void visualizeCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& filter_cloud) {
	//---------显示点云-----------------------
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("显示点云"));

	int v1(0), v2(0);
	viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer->setBackgroundColor(0, 0, 0, v1);
	viewer->addText("point clouds", 10, 10, "v1_text", v1);
	viewer->createViewPort(0.5, 0.0, 1, 1.0, v2);
	viewer->setBackgroundColor(0.1, 0.1, 0.1, v2);
	viewer->addText("filtered point clouds", 10, 10, "v2_text", v2);
	pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(cloud, "z"); // 按照z字段进行渲染,将z改为x或y即为按照x或y字段渲染
	viewer->addPointCloud<pcl::PointXYZ>(cloud, fildColor, "sample cloud", v1);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,0,0,1,"sample cloud",v1);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,5,"sample cloud",v1);

        pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor_filtered(cloud, "z"); // 按照z字段进行渲染,将z改为x或y即为按照x或y字段渲染
	viewer->addPointCloud<pcl::PointXYZ>(filter_cloud, fildColor_filtered,"cloud_filtered", v2);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,0,1,0,"cloud_filtered",v2);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,5,"cloud_filtered",v2);
	//viewer->addCoordinateSystem(1.0);
	//viewer->initCameraParameters();
    //viewer->setCameraPosition(0, 0, -1, 0, -1, 0, 0); // 设置相机位置
    // pcl::visualization::Camera camera;
    // bool hasSavedCameraParameters = false;
    // if (std::ifstream("camera_parameters.txt"))
    // {
    //     loadCameraParameters("camera_parameters.txt", camera);
    //     hasSavedCameraParameters = true;
    //     viewer->setCameraParameters(camera);
    // }

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
          // 检查用户是否按下 's' 键保存相机参数
    //     if (viewer->wasStopped())
    //         saveCameraParameters("camera_parameters.txt", viewer->getCameraParameters());
    // }
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}

